#include "lfm.h"
#include "utils.h"
#include "base/geometry.h"
#include "base/log.h"
#include "base/fileUtils.h"
#include <output/output.h>
#include <opencv2/opencv.hpp>
#include <sstream>

#ifndef EPS
#define EPS 1e-12
#endif

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795;
#endif

#define DUMP

namespace LFM
{
    bool LaneMatching::Calculate_Pose(
        const DlFrame &dl,
        const std::vector<hdmap_lane>& hdmap_lanes,
		const GeoPoint &gps_point,
		Camera &cam,
		Pose &pose,
		double &rmse,
        DbgFrame* dbg_frame)
    {
        LOG("start lane full matching ...") << std::endl;
        std::vector<hdmap_lane> hdlanes = utils::LoadByMiddleLayer(gps_point, hdmap_lanes, 3.0, 100.0);
        if (hdlanes.empty())
        {
            LOG("end lane full matching, the hdlanes is empty ...") << std::endl;
            return false;
        }

        std::vector<ARSLane> arslanes;
        for (const DlLane &dlLane : dl.lanes)
        {
            std::vector<ARSLane> splines = utils::FittingSpec(dlLane, cam.get_K(), cam.get_D());
            arslanes.insert(arslanes.end(), splines.begin(), splines.end());
        }

        Eigen::Vector2d vanish_pt;
        if(utils::CalVanishPt(arslanes, vanish_pt))
        {
            dbg_frame->DrawRound2D(vanish_pt, cv::Scalar(0, 0, 255), 3.0);
            double update_pith = std::atan((vanish_pt.y() - cam.cy()) / cam.fy());
            Eigen::Vector3d cam_ypr;
			cam.mount.GetYpr(&cam_ypr);
			cam_ypr.y() = update_pith;
			cam.mount.SetYpr(cam_ypr);
        }

        if(!utils::Recontruction(arslanes, cam, cam.mount))
        {
            LOG("end lane full matching, the dlLanes is empty ...") << std::endl;
            return false;
        }

#ifdef DUMP
        utils::DumpSplines3D("./splines/" + FileUtils::base_name_part(gps_point.imagename) + ".txt", arslanes, hdlanes);
#endif

        Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
        Eigen::Vector3d T = Eigen::Vector3d::Zero();
        std::vector<accordance_pair> match_pair_res;
        if (utils::ComputeRT(arslanes, hdlanes, R, T, rmse, match_pair_res, 10.0))
        {
            double heading = gps_point.bearing;
            Eigen::Vector3d n(std::cos(heading), std::sin(heading), 0.0);
            n = n.normalized();

            Eigen::Vector3d u = n.cross(Eigen::Vector3d(0, 0, 1));
            u = u.normalized();

            Eigen::Vector3d v = n.cross(u);
            v = v.normalized();

            Eigen::Matrix3d wr;
            wr << n(0), u(0), v(0), // C2W
                n(1), u(1), v(1),
                n(2), u(2), v(2);

            Eigen::Vector3d wt;
            wt << gps_point.x,
                gps_point.y,
                gps_point.z;

            Eigen::Matrix3d lfmR = wr * R;
            Eigen::Vector3d lfmT = wr * T + wt;

            pose.position.x() = lfmT.x();
            pose.position.y() = lfmT.y();
            pose.position.z() = lfmT.z();
            pose.orientation = Eigen::Quaternion<double>(lfmR);

            Trans_Reconstruction(arslanes, lfmR, lfmT);
            DrawSplines(dbg_frame, arslanes);

            LOG("end lane full matching, the result is true... ") << std::endl;
            LOG(" RMSE : ") << rmse << std::endl;
            return true;
        }
        LOG("end lane full matching, the result is false ...") << std::endl;
        return false;
    }

    void LaneMatching::DrawSplines(DbgFrame* dbg_frame, const std::vector<ARSLane>& lanes)
    {
        for(ARSLane lane : lanes)
        {
            dbg_frame->DrawSolidLine3D(lane.geometry3D, cv::Scalar(0, 255, 0), 2);
        }
    }

    void LaneMatching::Trans_Reconstruction(std::vector<ARSLane>& lanes,
        const Eigen::Matrix3d& R, 
        const Eigen::Vector3d& T)
    {
        for(ARSLane& lane : lanes)
        {
            for(std::size_t i = 0; i < lane.geometry3D.size(); i++)
            {
                lane.geometry3D[i] = R * lane.geometry3D[i] + T;
            }
        }
    }

} // namespace LFM